#include <calibration/calibration.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>

Calibration::Calibration(int argc, char **argv) :
	measurements_per_distance(10),
	store_adc_values(false)
{
	ros::NodeHandle nh("calibration");

	adc_subscriber = nh.subscribe("/roboard/adc", 1, &Calibration::receiveADC, this);

	ros::Rate loop_rate(20);
	std::cout << "Enter the sensor number: ";
	std::cin >> sensor_num;

	float max_distance;
	std::cout << "Enter the max range of the sensor in centimeters: ";
	std::cin >> max_distance;

	float min_distance;
	std::cout << "Enter the min distance you can measure with this sensor in centimeters.";
	std::cin >> min_distance;

	min_distance *= 0.01f;
	max_distance *= 0.01f;

	std::cout << "Enter the distance step size in centimeters: ";
	std::cin >> distance_step;
	getchar();

	std::ofstream file;
	std::stringstream ss;
	ss << "calibration" << sensor_num;
	file.open(ss.str().c_str(), std::ios::out);

	float current_distance = min_distance;
	while (ros::ok()) {
		if (store_adc_values) {
			if (adc_values.size() >= measurements_per_distance) {
				int64_t cumulative = 0;
				for (unsigned int i = 0; i < adc_values.size(); i++) {
					cumulative += adc_values[i];
				}
				int mean = cumulative / adc_values.size();

				file << current_distance << " " << mean << std::endl;

				adc_values.clear();
				store_adc_values = false;

				current_distance += distance_step * 0.01f;
			}

			if (current_distance - 0.01f > max_distance) {
				std::cout << "Calibration outputted to " << ss.str() << std::endl;
				break;
			}
		} else {
			std::cout << "Set the distance to " << (current_distance * 100.0f) << " centimeters... [ENTER]";
			getchar();
			std::cout << "Measuring..." << std::endl;

			skip_first = true;
			store_adc_values = true;
		}

		// Allow callbacks to be called
		ros::spinOnce();

		// Limit loop rate by calling sleep()
		loop_rate.sleep();
	}

	file.close();
}


void Calibration::receiveADC(const roboard_drivers::adc_val::ConstPtr &msg)
{
	if (!store_adc_values) return;
	if (skip_first) {
		skip_first = false;
		return;
	}

	int val;
	switch (sensor_num) {
		case 0: val = msg->val0; break;
		case 1: val = msg->val1; break;
		case 2: val = msg->val2; break;
		case 3: val = msg->val3; break;
		case 4: val = msg->val4; break;
		case 5: val = msg->val5; break;
		case 6: val = msg->val6; break;
		case 7: val = msg->val7; break;
		default:
			printf("Sensor number invalid \n");
	}

	adc_values.push_back(val);
}

Calibration::~Calibration()
{

}

int main (int argc, char **argv)
{
  	ros::init(argc, argv, "calibration");
	new Calibration(argc, argv);
	return 0;
}
